#include <mc_path_planner/complete_coverage_path/edge_path.h>

namespace ccpp{

EdgePathPlanner::EdgePathPlanner(boost::shared_ptr<MapBase> map_handler_ptr, boost::shared_ptr<SmoothBase> smooth_ptr)
    :CoveragePlannerBase(map_handler_ptr, smooth_ptr){

}

EdgePathPlanner::~EdgePathPlanner(){

}
        
// 贴边路径生成
bool EdgePathPlanner::genEdgePathWithPolygon(geometry_msgs::Point start, vector<geometry_msgs::Point> polygon, CoveragePath& path){
    if(!mapHandlerPtr_){
        ROS_ERROR("please set map data!");
        return false;
    }

    if(!mapHandlerPtr_->getMapBinary(originMap_)){
        ROS_ERROR("get map data error!");
    }

    polygonInfo_.origin_start = start;
    polygonInfo_.start_pixels = Word2Opencv(start);
    polygonInfo_.origin_polygon = polygon;
    for(auto point : polygon){
        polygonInfo_.polygon_pixels.emplace_back(Word2Opencv(point));
    }
    cout << "start_pixels: " << polygonInfo_.start_pixels  << ", polygon size: " << polygon.size() << endl;

    // 调试模式
    if(debug_mode_){
        cv::Mat debug_img;
        cv::cvtColor(originMap_, debug_img, cv::COLOR_GRAY2BGR);
        // 绘制起点
        cv::circle(debug_img, polygonInfo_.start_pixels, 2, cv::Scalar(255, 0, 0), -1);
        // 绘制多边形
        cv::polylines(debug_img, polygonInfo_.polygon_pixels, true, cv::Scalar(0, 255, 0), 1, 8, 0);
        cv::imwrite(debugDir_ + "/polygon.png", debug_img);
    }

        
    // 提取多边形
    std::vector<std::vector<cv::Point>> mask_area;
    mask_area.emplace_back(polygonInfo_.polygon_pixels);
    cv::Mat polygon_mask_img;
    cv::Mat mask(originMap_.rows, originMap_.cols, CV_8UC1, cv::Scalar(0));
    cv::fillPoly(mask, mask_area, cv::Scalar(255));
    originMap_.copyTo(polygon_mask_img, mask);

    // 调试模式
    if(debug_mode_){
        cv::imwrite(debugDir_ + "/mask.png", mask);
        cv::imwrite(debugDir_ + "/polygon_mask.png", polygon_mask_img);
    }

    // 形态学操作
    int sz = (int)std::round(((float)coveragePathConfig_.path_width) / mapHandlerPtr_->getResolution());
    cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(sz, sz), cv::Point(-1, -1)); // size: robot radius
    cv::erode(polygon_mask_img, polygon_mask_img, erode_kernel);

    // 调试模式
    if(debug_mode_){
        cv::imwrite(debugDir_ + "/polygon_mask_erode.png", polygon_mask_img);
    }

    // 提取轮廓
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
    cv::findContours(polygon_mask_img, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE);

    // 计算父子轮廓索引
    std::map<int, vector<cv_points>> order_contours;
    // 遍历每个轮廓
    for (size_t i = 0; i < contours.size(); ++i) {
        vector<cv::Point> approx;
        // 设置 epsilon 为周长的 2%（可调节）
        // double epsilon = 0.02 * cv::arcLength(contours[i], true);
        cv::approxPolyDP(contours[i], approx, 2, true);     //多边形逼近，优化像素错位导致摆头现象
        
        int parent_idx = hierarchy[i][3]; // 父轮廓索引
        if (parent_idx == -1) {
            // 没有父轮廓（顶层轮廓）
            order_contours[i].emplace_back(approx);
        } else {
            // 将当前轮廓添加到父轮廓的子轮廓列表中
            order_contours[parent_idx].emplace_back(approx);
        }
    }

    // 路径后处理
    for (const auto contours : order_contours) {
        for(size_t i = 0; i < contours.second.size(); ++i){
            // 对每条轮廓进行处理
            vector<Point2f> contour_points_2f;
            for(auto contour_point : contours.second[i]){
                // 特征点转换为世界坐标
                contour_points_2f.emplace_back(mapHandlerPtr_->OpencvPixelToWorld<cv::Point, Point2f>(contour_point));
            }
            // 首尾相连
            if(!contours.second[i].empty())
                contour_points_2f.emplace_back(mapHandlerPtr_->OpencvPixelToWorld<cv::Point, Point2f>(contours.second[i].front()));
            path.edge_path.emplace_back(contour_points_2f);


            // 特征点之间填充直线数据
            // vector<Point2f> filled_points_2f = fillAllPoints(contour_points_2f, 0.05);

            // 路径平滑处理
            // vector<Point2f> smooth_points_2f;
            // smoothPathPtr_->smoothPath(filled_points_2f, smooth_points_2f);
            // path.edge_path.emplace_back(smooth_points_2f);
        }
    }


    // // 获取轮廓面积进行排序和离起点最近的点
    // vector<ContourInfo> contours_info;
    // for(int i=0; i< contours.size(); ++i){
    //     ContourInfo contour_info;

    //     // // 轮廓平滑处理
    //     // vector<cv::Point> approx;
    //     // cv::approxPolyDP(contours[i], approx, 2, true);
    //     // contour_info.contour = approx;

    //     contour_info.contour = contours[i];
    //     // 计算面积
    //     contour_info.area = cv::contourArea(contour_info.contour);
    //     if(!contour_info.contour.empty()){
    //         // 判断起点是否在轮廓内
    //         contour_info.start_in_contour = false;
    //         if((pointPolygonTest(contour_info.contour, polygonInfo_.start_pixels, false) >= 0)){
    //             contour_info.start_in_contour = true;
    //         }
    //         if(contour_info.start_in_contour){
    //             // 计算离起点最近的点
    //             int min_index = -1;
    //             double min_value = INFINITY;
    //             for(int n=0; n<contour_info.contour.size(); n++){
    //                 auto contour_point = contour_info.contour[n];
    //                 double dis = calculateDistance(polygonInfo_.start_pixels, contour_point);
    //                 if(dis < min_value){
    //                     min_value = dis;
    //                     min_index = n;
    //                 }
    //             }
    //             contour_info.nearest_start_point = make_pair(min_index, contour_info.contour[min_index]);                
    //         }
    //     }
    //     contours_info.emplace_back(contour_info);
    // }


    // // // 按轮廓面积进行排序
    // // sort(contours_info.begin(), contours_info.end(), [](ContourInfo a, ContourInfo b){
    // //     return a.area > b.area;
    // // });


    // // 调试模式
    // if(debug_mode_){
    //     cv::Mat debug_img = cv::Mat(originMap_.rows, originMap_.cols, CV_8UC3, cv::Scalar(255, 255, 255));
    //     // 绘制多边形
    //     cv::polylines(debug_img, polygonInfo_.polygon_pixels, true, cv::Scalar(0, 255, 0), 1, 8, 0);
    //     // 绘制起点
    //     cv::circle(debug_img, polygonInfo_.start_pixels, 2, cv::Scalar(255, 0, 0), -1);
    //     // 绘制轮廓
    //     cv::drawContours(debug_img, contours, -1, cv::Scalar(255, 0, 0), 1);
    //     cv::imwrite(debugDir_ + "/polygon_contours_draw.png", debug_img);
    //     // 绘制点集
    //     // for(int i=0; i< contours_info.size(); ++i){
    //     for (const auto contours : order_contours) {
    //         for(size_t i = 0; i < contours.second.size(); ++i){
    //             // cv::polylines(debug_img, contours.second, true, cv::Scalar(0, 255, 0), 2);
    //             for(auto point : contours.second[i]){
    //                 cv::circle(debug_img, point, 2, cv::Scalar(0, 0, 255), -1);
    //             }
    //         }
    //     }
    //     cv::imwrite(debugDir_ + "/polygon_points_draw.png", debug_img);
    // }

    // // 简单输出贴边路径
    // // for (const auto& [parent_idx, children] : order_contours) {
    // //     std::cout << "Parent Contour Index: " << parent_idx << std::endl;
    // //     for (size_t i = 0; i < children.size(); ++i) {
    // //         std::cout << "  Child " << i << ": " << children[i].size() << " points" << std::endl;
    // //     }
    // // }

    
    return true;
}

};  //namespace